#pragma once

template <typename T>
auto distance(T x1, T y1, T x2, T y2) -> T
{
    T dx = x1-x2;
    T dy = y1-y2;
    return sqrt(dx*dx + dy*dy);
}

template <typename T>
inline void closest_point_on_segment_from_point(const T& x1, const T& y1,
                                                const T& x2, const T& y2,
                                                const T& px, const T& py,
                                                T& nx,       T& ny)
{
    const T vx = x2 - x1;
    const T vy = y2 - y1;
    const T wx = px - x1;
    const T wy = py - y1;

    const T c1 = vx * wx + vy * wy;

    if (c1 <= T(0.0))
    {
        nx = x1;
        ny = y1;

        return;
    }

    const T c2 = vx * vx + vy * vy;

    if (c2 <= c1)
    {
        nx = x2;
        ny = y2;

        return;
    }

    const T ratio = c1 / c2;

    nx = x1 + ratio * vx;
    ny = y1 + ratio * vy;
}